#include <rclcpp/rclcpp.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <yaml-cpp/yaml.h>
#include <fstream>

class TagVisualizer : public rclcpp::Node
{
public:
    TagVisualizer() : Node("tag_visualizer")
    {
        // 创建发布器
        marker_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("tag_markers", 10);

        // 定时器，用于定期加载 YAML 文件并发布标记
        timer_ = this->create_wall_timer(
            std::chrono::seconds(1),
            std::bind(&TagVisualizer::loadAndPublishMarkers, this));
    }

private:
    void loadAndPublishMarkers()
    {
        // 加载 YAML 文件
        YAML::Node yaml_data;
        try
        {
            std::ifstream fin("/home/fwy/nav_ws/src/fishbot_apriltag_detect/tag_save_map/tag_tf_map.yaml");
            if (!fin.is_open())
            {
                RCLCPP_ERROR(this->get_logger(), "Failed to open tag_tf_map.yaml");
                return;
            }
            yaml_data = YAML::Load(fin);
            fin.close();
        }
        catch (const std::exception &e)
        {
            RCLCPP_ERROR(this->get_logger(), "Failed to parse YAML file: %s", e.what());
            return;
        }

        // 创建 MarkerArray
        visualization_msgs::msg::MarkerArray marker_array;

        // 遍历 YAML 数据
        for (const auto &tag : yaml_data)
        {
            if (tag.first.as<std::string>() == "header")
            {
                // 跳过 header 信息
                continue;
            }

            const std::string tag_name = tag.first.as<std::string>();
            const auto &translation = tag.second["translation"];
            const auto &rotation = tag.second["rotation"];

            // 创建一个 Marker
            visualization_msgs::msg::Marker marker;
            marker.header.frame_id = "map"; // 在 map 坐标系下可视化
            marker.header.stamp = this->now();
            marker.ns = "tags";
            marker.id = marker_array.markers.size(); // 使用数组索引作为 ID
            marker.type = visualization_msgs::msg::Marker::CUBE; // 使用长方体表示 tag
            marker.action = visualization_msgs::msg::Marker::ADD;

            // 设置位置
            marker.pose.position.x = translation["x"].as<double>();
            marker.pose.position.y = translation["y"].as<double>();
            marker.pose.position.z = translation["z"].as<double>();

            // 设置方向（四元数）
            marker.pose.orientation.x = rotation["x"].as<double>();
            marker.pose.orientation.y = rotation["y"].as<double>();
            marker.pose.orientation.z = rotation["z"].as<double>();
            marker.pose.orientation.w = rotation["w"].as<double>();

        
            // 设置标记的大小
            marker.scale.x = 0.2; // 半径为 0.2 米
            marker.scale.y = 0.2;
            marker.scale.z = 0.2;

            // 设置颜色
            marker.color.r = 1.0; // 红色
            marker.color.g = 0.0;
            marker.color.b = 0.0;
            marker.color.a = 1.0; // 不透明

            // 添加文本标记
            visualization_msgs::msg::Marker text_marker = marker;
            text_marker.id = marker.id + 1000; // 确保 ID 唯一
            text_marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
            text_marker.text = tag_name; // 显示 tag 的名字
            text_marker.pose.position.z += 0.3; // 文本位置稍微高于球体
            text_marker.scale.z = 0.2; // 文本大小
            text_marker.color.r = 0.0;
            text_marker.color.g = 1.0; // 绿色文本
            text_marker.color.b = 0.0;

            // 添加到 MarkerArray
            marker_array.markers.push_back(marker);
            marker_array.markers.push_back(text_marker);
        }

        // 发布 MarkerArray
        marker_pub_->publish(marker_array);
        RCLCPP_INFO(this->get_logger(), "Published %zu markers", marker_array.markers.size());
    }

    rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr marker_pub_;
    rclcpp::TimerBase::SharedPtr timer_;
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<TagVisualizer>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}